#include "fifo.h"


void FIFO_Init(FIFO_t *fifo) {
    fifo->head = 0;
    fifo->tail = 0;
    fifo->count = 0;
}

// Push messages into FIFO; Returns -1 if the queue is full
int FIFO_Push(FIFO_t *fifo, const SensorMsg_t *msg) {
    if (fifo->count >= FIFO_SIZE) {
        return -1;
    }
    fifo->buffer[fifo->tail] = *msg;
    fifo->tail = (fifo->tail + 1) % FIFO_SIZE;
    fifo->count++;
    return 0;
}

// Pop-up message from FIFO; Returns -1 if the queue is empty
int FIFO_Pop(FIFO_t *fifo, SensorMsg_t *msg) {
    if (fifo->count == 0) {
        return -1;
    }
    *msg = fifo->buffer[fifo->head];
    fifo->head = (fifo->head + 1) % FIFO_SIZE;
    fifo->count--;
    return 0;
}

// Get size of FIFO
int FIFO_GetSize(FIFO_t *fifo) {

    return fifo->count;
}
